#include <unionsys_core.hpp>
#include <iostream>  


class UnionsysEKF{  
public:  
    UnionsysEKF();
    void predict();
    void update(const Eigen::VectorXd& z);
    Eigen::VectorXd getState() const {  
        return x_;  
    }  
    Eigen::VectorXd getK_() const {  
        return K_;  
    }    
private:  
    Eigen::VectorXd x_;  
    Eigen::MatrixXd P_; 
    Eigen::MatrixXd F_;  
    Eigen::MatrixXd H_;   
    Eigen::MatrixXd R_;   
    Eigen::MatrixXd Q_;  
    Eigen::MatrixXd K_; 
};

